
#include <stdbool.h>
#include <string.h>

#include "algo_pedometer.h"
#include "algo_flip_wrist_gesture.h"


#include "..\log\log.h"

#include "square_root.h"

#define LOG     nLOG_INFO 
#define LOG3    LOG_INFO3 

#define RAW_BUFF_SIZE           (4)
#define RAW_BUFF_SIZE_MASK      (RAW_BUFF_SIZE - 1)

#define VECTOR_BUFF_SIZE        (15)
#define VECTOR_BUFF_SIZE_MASK   (VECTOR_BUFF_SIZE - 1)

typedef enum{
    ALGO_STEP_ACT_STATE_INIT = 0,
    ALGO_STEP_ACT_STATE_FIND_LOW, 
    ALGO_STEP_ACT_STATE_FIND_PEAK,    
}algo_step_act_state_t;

typedef struct{
    uint32_t    value_of_max;
    uint32_t    value_of_min;
    uint32_t    index_of_max;
    uint32_t    index_of_min;
    
    uint32_t    idx; 
    uint32_t    value[VECTOR_BUFF_SIZE]; //80bytes    
}algo_vector_t;

typedef struct{
    uint32_t    possibleSteps; 
    uint32_t    last_min_value_of_vector; 

    algo_step_act_state_t state; 
    int         flag_max_to_max; 
    int         interval_of_min_to_max; 
    int         flag_interval_of_step; 

    int         flag_regulate;
    int         invalid_count; 
}algo_activity_t; 

typedef struct{
    int         count;
}algo_static_t; 

typedef struct{
    uint8_t         detected;
    uint8_t         count;
    uint8_t         direction; 
    uint8_t         enabled; 
}algo_raise_action_t; 


////////////////////////////////////////////////////////////////////////////////////////
static algo_axis_data_t axis_raw[RAW_BUFF_SIZE]; 
static int indexOfWindowRaw; 

static algo_axis_data_t filter;
static algo_axis_data_t axis_average;
static algo_vector_t    m_vector; 
static algo_activity_t  m_algo_act; 

// static algo_raise_up_evt_handle_t m_pedo_algo_raise_up_callback = NULL;


static algo_static_t    m_algo_static; 
static int m_count_of_unregulate = 0 ;
static int m_count_of_invalid = 0;

static uint32_t steps_of_temp = 0;


////////////////////////////////////////////////////////////////////////////////////////



// void algo_pedo_raise_up_cb_register(algo_raise_up_evt_handle_t cb )
// {
//     if( m_pedo_algo_raise_up_callback == NULL ) 
//     {
//         m_pedo_algo_raise_up_callback = cb;
//     }
// }

static void algo_pedo_on_raise_up_update(bool actived)
{
    algo_evt_t evt;
    evt.type = actived ? ALGO_EVT_TYPE_RAISED_ACTIVE : ALGO_EVT_TYPE_RAISED_REMOVE;

    m_algo_callback( &evt );
}
static inline void algo_step_activity_step_change_cb(int step)
{
    algo_evt_t evt;
    evt.type = ALGO_EVT_TYPE_STEPS;
    evt.params.steps = step;
    m_algo_callback( &evt); 

    steps_of_temp += step;
}

static inline void algo_step_activity_static_change_cb(void)
{
    algo_evt_t evt;
    evt.type = ALGO_EVT_TYPE_STATIC;
    m_algo_callback( & evt ); 
}
static inline void algo_step_activity_change_cb(void)
{
    algo_evt_t evt;
    evt.type = ALGO_EVT_TYPE_ACTIVITY;
    m_algo_callback( & evt ); 
}

void algo_step_activity_init(void)
{
    memset( &m_algo_act, 0, sizeof(m_algo_act));
    memset( &m_vector, 0, sizeof(m_vector)); 
    memset( &axis_raw, 0, sizeof(axis_raw));
    memset( &filter, 0, sizeof(filter));
    memset( &axis_average, 0, sizeof(axis_average)); 

    indexOfWindowRaw = 0;    
}

//////////////////////////////////////////////////////////////////////////
////// algorithm main functions 
//////////////////////////////////////////////////////////////////////////
static void algo_filter_average(const algo_axis_data_t * p_axis_data)
{
    // average filter 
    axis_average.x = (int16_t)(axis_average.x - axis_raw[indexOfWindowRaw].x + p_axis_data->x);
    axis_average.y = (int16_t)(axis_average.y - axis_raw[indexOfWindowRaw].y + p_axis_data->y);
    axis_average.z = (int16_t)(axis_average.z - axis_raw[indexOfWindowRaw].z + p_axis_data->z);

    filter.x = (int16_t)(axis_average.x  / RAW_BUFF_SIZE);
    filter.y = (int16_t)(axis_average.y  / RAW_BUFF_SIZE);
    filter.z = (int16_t)(axis_average.z  / RAW_BUFF_SIZE);

    // save curernt raw data 
    axis_raw[indexOfWindowRaw].x = p_axis_data->x;
    axis_raw[indexOfWindowRaw].y = p_axis_data->y;
    axis_raw[indexOfWindowRaw].z = p_axis_data->z;

    indexOfWindowRaw ++;
    if(indexOfWindowRaw > RAW_BUFF_SIZE_MASK)
    {
        indexOfWindowRaw = 0; 
    }
    
}

static void algo_window_find_max_min(void)
{
    // compute m_vector of 3-axis 
    uint32_t v = (uint32_t)(filter.x * filter.x + filter.y * filter.y + filter.z * filter.z);
    v = SquareRoot(v);
    m_vector.value[m_vector.idx] = v;
    m_vector.idx ++;
    if(m_vector.idx > VECTOR_BUFF_SIZE_MASK)
    {
        m_vector.idx = 0; 
    }

    m_vector.index_of_max = m_vector.index_of_min = 0;
    m_vector.value_of_max = m_vector.value_of_min = m_vector.value[0];

    for(int i = 1; i < VECTOR_BUFF_SIZE; i ++)
    {
        if( m_vector.value_of_max < m_vector.value[i])
        {
            m_vector.value_of_max = m_vector.value[i];
            m_vector.index_of_max = i; 
        }
        if( m_vector.value_of_min > m_vector.value[i])
        {
            m_vector.value_of_min = m_vector.value[i];
            m_vector.index_of_min = i; 
        }
    }
}

static void algo_step_invalid_timeout(void)
{
    m_algo_act.invalid_count ++;
    if( m_algo_act.invalid_count > 3)
    {
        m_algo_act.invalid_count = 0;
        m_algo_act.flag_regulate = 0; 
    }
}
static void algo_step_under_regulate_mode(void)
{
    int32_t diff = m_vector.value_of_max - m_algo_act.last_min_value_of_vector;
    if( diff > ALGO_STEP_THRESHOLD_HIGH)
    {
        if( m_algo_act.flag_interval_of_step >= ALGO_TIMER_TICK_MS(200) 
        &&  m_algo_act.flag_interval_of_step <= ALGO_TIMER_TICK_MS(1000)
        )
        {
            m_algo_act.flag_interval_of_step = 0;

            algo_step_activity_step_change_cb( 1 + m_algo_act.possibleSteps);
            m_algo_act.possibleSteps = 0;
            m_algo_act.invalid_count = 0;                  
        }
        else if(m_algo_act.flag_interval_of_step > ALGO_TIMER_TICK_MS(1000))
        {
            m_algo_act.flag_interval_of_step = 0;
            m_algo_act.possibleSteps = 1;
            algo_step_invalid_timeout();
        }
        else
        {
            algo_step_invalid_timeout();
        }
    }
    else if( diff > ALGO_STEP_THRESHOLD_MID)
    {
        if( 
            m_algo_act.flag_interval_of_step >= ALGO_TIMER_TICK_MS(250)
        &&  m_algo_act.flag_interval_of_step <= ALGO_TIMER_TICK_MS(1200)
         )
        {            
            m_algo_act.flag_interval_of_step = 0;
            algo_step_activity_step_change_cb( 1 + m_algo_act.possibleSteps);
            m_algo_act.possibleSteps = 0;
            m_algo_act.invalid_count = 0;              
        }
        else if( m_algo_act.flag_interval_of_step > ALGO_TIMER_TICK_MS(1200))
        {
            m_algo_act.flag_interval_of_step = 0;
            m_algo_act.possibleSteps = 1;
            algo_step_invalid_timeout();
        }
        else
        {
            algo_step_invalid_timeout();
        }
    }
    else if( diff > ALGO_STEP_THRESHOLD_LOW)
    {
        if( m_algo_act.flag_interval_of_step >= ALGO_TIMER_TICK_MS(300)
        &&  m_algo_act.flag_interval_of_step <= ALGO_TIMER_TICK_MS(1500)
        )
        {
            m_algo_act.flag_interval_of_step = 0;  
            algo_step_activity_step_change_cb( 1 + m_algo_act.possibleSteps);
            m_algo_act.possibleSteps = 0;
            m_algo_act.invalid_count = 0;             
        }
        else if(m_algo_act.flag_interval_of_step > ALGO_TIMER_TICK_MS(1500))
        {
            m_algo_act.flag_interval_of_step = 0;
            m_algo_act.possibleSteps = 1;
            algo_step_invalid_timeout();
        }
        else
        {
            algo_step_invalid_timeout();
        }
    }
    else
    {
        m_algo_act.possibleSteps = 0;
        algo_step_invalid_timeout();
    }
}
static void algo_step_under_unregulate_mode(void)
{
    int32_t diff = m_vector.value_of_max - m_algo_act.last_min_value_of_vector;

    m_count_of_invalid ++;
    // if m_vector too high, means range is width, device must under moving fast mode, run fast 
    if( diff > ALGO_STEP_THRESHOLD_HIGH)
    {
        m_count_of_invalid = 0;
        m_count_of_unregulate = 0;
        if( m_algo_act.flag_interval_of_step >= ALGO_TIMER_TICK_MS(180) 
        &&  m_algo_act.flag_interval_of_step <= ALGO_TIMER_TICK_MS(2000)
        )
        {
            m_algo_act.flag_interval_of_step = 0; 
            m_algo_act.possibleSteps ++;
            if( m_algo_act.possibleSteps >= ALGO_STEP_THRESHOLD) 
            {
                algo_step_activity_step_change_cb( m_algo_act.possibleSteps );
                m_algo_act.possibleSteps = 0; 
                m_algo_act.flag_regulate  = 1; 
            }
        } 
        else
        {
            m_algo_act.possibleSteps = 1;
            m_algo_act.flag_interval_of_step = 0;
        }
    }
    else if( diff > ALGO_STEP_THRESHOLD_MID) //maybe walk fast 
    {
        m_count_of_invalid = 0;
        m_count_of_unregulate = 0;
        if( m_algo_act.flag_interval_of_step >= ALGO_TIMER_TICK_MS(180) 
        &&  m_algo_act.flag_interval_of_step <= ALGO_TIMER_TICK_MS(1500)
        )
        {
            m_algo_act.flag_interval_of_step = 0; 
            m_algo_act.possibleSteps ++;
            if( m_algo_act.possibleSteps >= ALGO_STEP_THRESHOLD) 
            {
                algo_step_activity_step_change_cb( m_algo_act.possibleSteps );
                m_algo_act.possibleSteps = 0; 
                m_algo_act.flag_regulate  = 1; 
            }
        }
        else
        {
            m_algo_act.possibleSteps = 1;
            m_algo_act.flag_interval_of_step = 0;
        }
    }
    else if( diff > ALGO_STEP_THRESHOLD_LOW) // walk normal 
    {
        m_count_of_invalid = 0;
        m_count_of_unregulate = 0;
        if( m_algo_act.flag_interval_of_step >= ALGO_TIMER_TICK_MS(180)
        &&  m_algo_act.flag_interval_of_step <= ALGO_TIMER_TICK_MS(1500)
        )
        {
            m_algo_act.flag_interval_of_step = 0; 
            m_algo_act.possibleSteps ++;
            if( m_algo_act.possibleSteps >= ALGO_STEP_THRESHOLD) 
             {
                algo_step_activity_step_change_cb( m_algo_act.possibleSteps );
                m_algo_act.possibleSteps = 0; 
                m_algo_act.flag_regulate  = 1; 
            }
        }
        else{
            m_algo_act.possibleSteps = 1;
            m_algo_act.flag_interval_of_step = 0;
        }
    }
    else
    {
        m_count_of_unregulate ++;
        if( m_count_of_unregulate > 4 && m_count_of_invalid > ALGO_TIMER_TICK_MS(1500))
        {
            m_algo_act.possibleSteps = 0; 
            m_algo_act.flag_interval_of_step = 0;   
        }       
    }
}
static void algo_step_detection(void)
{
    // record step interval 
    m_algo_act.flag_interval_of_step ++;

    switch(m_algo_act.state)
    {
        case ALGO_STEP_ACT_STATE_INIT:
        if( m_vector.index_of_max != ((m_vector.idx + (VECTOR_BUFF_SIZE >> 1)) % VECTOR_BUFF_SIZE))
            break;
        
        m_algo_act.state = ALGO_STEP_ACT_STATE_FIND_LOW;
        m_algo_act.flag_max_to_max = 0; //flag max to max reset 
        break;

        case ALGO_STEP_ACT_STATE_FIND_LOW:
        if( m_vector.index_of_min != ((m_vector.idx + (VECTOR_BUFF_SIZE >> 1)) % VECTOR_BUFF_SIZE))
            break;

        m_algo_act.state = ALGO_STEP_ACT_STATE_FIND_PEAK;
        m_algo_act.last_min_value_of_vector = m_vector.value_of_min; //storage last min of window 
        break; 

        case ALGO_STEP_ACT_STATE_FIND_PEAK:
        {
            if( m_vector.index_of_max != ((m_vector.idx + (VECTOR_BUFF_SIZE >> 1)) % VECTOR_BUFF_SIZE))
                break;

            m_algo_act.state = ALGO_STEP_ACT_STATE_FIND_LOW;        

            // if winMax - winMin > threshold, activity count increase 
            int diff = m_vector.value_of_max - m_vector.value_of_min ;
            if( diff > ALGO_ACTIVITY_THRESHOLD)
            {
                //m_algo_act.activities ++; 
                algo_step_activity_change_cb(); 
            }

            if( m_vector.value_of_max - m_vector.value_of_min < ALGO_STATIC_THRESHOLD)
            {
                m_algo_static.count ++;      
                if( m_algo_static.count > 5)
                {
                    m_algo_static.count = 0; 
                    algo_step_activity_static_change_cb();
                }      
            } 
            else
            {
                m_algo_static.count = 0; 
            }
            

            //regulate mode 
            if( m_algo_act.flag_regulate > 0)
            { 
                algo_step_under_regulate_mode();
            }
            else
            {
                algo_step_under_unregulate_mode(); 
            }
            
            m_algo_act.flag_max_to_max = 0;
        }
        break;
        default:
        m_algo_act.state = ALGO_STEP_ACT_STATE_INIT; 
        break;        
    }

    //exit flag regulate 
    if( m_algo_act.flag_max_to_max ++ >= ALGO_TIMER_TICK_MS(1000))
    {
        m_algo_act.flag_max_to_max = 0; 
        m_algo_act.flag_regulate = 0; 
    }

}


void algo_step_activity_data_process(const algo_axis_data_t * p_axis_data)
{
//    LOG3("x=%d,y=%d,z=%d", p_axis_data->x, p_axis_data->y, p_axis_data->z );
    
    // filter 
    algo_filter_average( p_axis_data ); 

    // m_vector min & max 
    algo_window_find_max_min();

    // pedometer 
    algo_step_detection(); 

    // 翻腕算法
    algo_flip_wrist_gesture_recognise( filter.x, filter.y, filter.z );
}





